// Copyright (c) 2024, lfr
// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <limits>
#include <vector>

#include "leg_motor_hardware_interface/leg_motor_hardware_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace leg_motor_hardware_interface
{
CallbackReturn LegMotorHardwareInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
    return CallbackReturn::ERROR;
  }

  // TODO(anyone): read parameters and initialize the hardware
  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  return CallbackReturn::SUCCESS;
}

CallbackReturn LegMotorHardwareInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
  leg_motor_.init("can0");
  return CallbackReturn::SUCCESS;
}


std::vector<hardware_interface::StateInterface> LegMotorHardwareInterface::export_state_interfaces()
{
  
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    // 状态接口初始化为0.0
    hw_states_[i] = 0.0;
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> LegMotorHardwareInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i]));
  }

  return command_interfaces;
}

CallbackReturn LegMotorHardwareInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to receive commands
  if(!leg_motor_.leg_motor_start())
  {
    return CallbackReturn::ERROR;
  }
  // 初始控制命令为0
  hw_commands_[0] = 0.0;
  return CallbackReturn::SUCCESS;
}

CallbackReturn LegMotorHardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to stop receiving commands
  if(!leg_motor_.leg_motor_shutdown())
  {
    return CallbackReturn::ERROR;
  }
  return CallbackReturn::SUCCESS;
}

hardware_interface::return_type LegMotorHardwareInterface::read()
{
  // TODO(anyone): read robot states
  hw_states_[0] = leg_motor_.leg_motor_position_read();
  double effort = leg_motor_.leg_motor_effort_read();
  RCLCPP_INFO(rclcpp::get_logger("leg_motor"), "position: %f, effort: %f\n", hw_states_[0], effort);
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type LegMotorHardwareInterface::write()
{
  // TODO(anyone): write robot's commands'
  leg_motor_.leg_motor_effort_control(hw_commands_[0]);
  return hardware_interface::return_type::OK;
}

}  // namespace leg_motor_hardware_interface

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  leg_motor_hardware_interface::LegMotorHardwareInterface, hardware_interface::SystemInterface)
